#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>

#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/builtin_bool.h>

#define KEYCODE_BEGIN 0x62//b
#define KEY_UP	0x26
#define KEY_DOWN	0x28
#define KEY_LEFT	0x25
#define KEY_RIGHT	0x27

#define KEYCODE_W 0x77
#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64

#define KEYCODE_A_CAP 0x41
#define KEYCODE_D_CAP 0x44
#define KEYCODE_S_CAP 0x53
#define KEYCODE_W_CAP 0x57


#define KEYCODE_0 0x30
#define KEYCODE_1 0x31
#define KEYCODE_2 0x32
#define KEYCODE_3 0x33
#define KEYCODE_4 0x34
#define KEYCODE_5 0x35
#define KEYCODE_6 0x36
#define KEYCODE_7 0x37
#define KEYCODE_8 0x38
#define KEYCODE_9 0x39

class SmartCarKeyboardTeleopNode
{
    private:

        float  x_sp;
        float  y_sp;
        float  z_sp;
        float  yaw_sp;//%degree

        float liner_v;
        float rotate_v;

        float x_bias;
        float y_bias;
        float z_bias;
        float R_bias;
        float rotate_speed_bias;

        bool reset;

        geometry_msgs::PoseStamped pose_sp_keyboard;// from keyboad commmand
        geometry_msgs::TwistStamped vel_modify_keyboard;// from keyboad commmand
        ros::NodeHandle nh;
        ros::Publisher pose_set_pub;
        ros::Publisher vel_modify_set_pub;

        ros::Subscriber uavpose_sub;
        ros::Subscriber key_mission;
        geometry_msgs::PoseStamped pose_init;

        bool StateIsInit;
        bool StartOffControl;

    public:
        SmartCarKeyboardTeleopNode()
        {
        	StateIsInit = true;
        	StartOffControl =true;
        	pose_set_pub = nh.advertise<geometry_msgs::PoseStamped>("keyboard_command_pos",10);
        	vel_modify_set_pub = nh.advertise<geometry_msgs::TwistStamped>("keyboard_command",10);
        	//
        	//another style
        	ros::NodeHandle nPrivate("~");
        	nPrivate.param("x_sp",x_sp,0.0f);
        	nPrivate.param("y_sp",y_sp,0.0f);
        	nPrivate.param("z_sp",z_sp,0.0f);
        	nPrivate.param("yaw_sp",yaw_sp,0.0f);

        	liner_v = 0.0f;
        	rotate_v = 0.0f;

            x_bias = 0.0;
            y_bias = 0.0;
            z_bias = 0.0;
            R_bias = 0.0;
            rotate_speed_bias = 0.0;
            reset = false;
            uavpose_sub = nh.subscribe("/mavros/local_position/pose", 10, &SmartCarKeyboardTeleopNode::getPose_cb,this);
            key_mission = nh.subscribe("Key_mission", 10, &SmartCarKeyboardTeleopNode::getkey_cb,this);
        }

        ~SmartCarKeyboardTeleopNode() { }
        void keyboardLoop();
        void getkey_cb(const boost::shared_ptr<bool const>& msg)
        {
        	ROS_INFO("getkey_cb, msg = %d\n", *msg);
        	bool new_msg =*msg;
        		if(new_msg)
        		{
        			reset = true;
        		}
        		else
        		{
        			reset = false;
        		}
        }
        void getPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
        {

        	pose_init =*msg;
        	StateIsInit = true;
//early function
//        	pose_init =*msg;
//        	StateIsInit = true;
//        	if(!StartOffControl)
//        	{
//        //		pose_sp_keyboard=pose_init;
//
//        		x_sp = pose_init.pose.position.x;
//        		y_sp = pose_init.pose.position.y;
//        		z_sp = pose_init.pose.position.z;
//
//        		pose_sp_keyboard.pose.orientation = pose_init.pose.orientation;
//        	}

        }

        void stopRobot()
        {
        	StartOffControl = false;
        	StateIsInit = false;


        }
};

SmartCarKeyboardTeleopNode* tbk;
int kfd = 0;
struct termios cooked, raw;
bool done;

int main(int argc, char** argv)
{
    ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
    SmartCarKeyboardTeleopNode tbk;

    boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));

    ros::spin();

    t.interrupt();
    t.join();
    tbk.stopRobot();

    tcsetattr(kfd, TCSANOW, &cooked);

    return(0);
}

void SmartCarKeyboardTeleopNode::keyboardLoop()
{


    bool dirty = false;


    // get the console in raw mode
    tcgetattr(kfd, &cooked);
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &=~ (ICANON | ECHO);//使用标准控制模式输入 | 显示输入字符
    raw.c_cc[VEOL] = 1; 	//附加的End-of-file字符
    raw.c_cc[VEOF] = 2;		//End-of-file字符

    //    c_cc[VEOL]和c_cc[VEOL2]
    //    在标准模式下，这两个下标在行的末尾加上一个换行符（'/n'），标志一个行的结束，从而使用缓冲区中的数据被发送，并开始新的一行。POSIX中 没有定义VEOL2。

    //    c_cc[VEOF]
    //    默认对应的控制符是^D，在标准模式下，使用read()返回0，标志一个文件结束。
    //    TCSANOW：不等数据传输完毕就立即改变属性。
    //    TCSADRAIN：等待所有数据传输结束才改变属性。
    //    TCSAFLUSH：清空输入输出缓冲区才改变属性。
    tcsetattr(kfd, TCSANOW, &raw);

    puts("Reading from keyboard");
    puts("Use WASD keys to control the robot");
    puts("Press Shift to move faster");

    struct pollfd ufd;
    ufd.fd = kfd;

    ROS_INFO("kfd=%d\n",kfd);


    ufd.events = POLLIN;




    //for debug
   // StateIsInit = true;
   // StartOffControl =true;
    static int co = 0;
    for(;;)
    {
        char c =0x00;

        boost::this_thread::interruption_point();

        if(!StateIsInit)
        	continue;

        //ROS_INFO("StateIsInit=%d\n",StateIsInit);
        //must set the pos
        // get the next event from the keyboard
        int num;

        if ((num = poll(&ufd, 1, 25)) < 0)
        {
            perror("poll():");
            return;
        }
        else if(num > 0)
        {
            if(read(kfd, &c, 1) < 0)
            {
                perror("read():");
                return;
            }
        }
        else
        {
//            if (dirty == true)
//            {
//                stopRobot();
//                dirty = false;
//            }
//
//            continue;
        }


//        if(!StartOffControl&&c==KEYCODE_BEGIN)
//        {
//        	ROS_INFO("OffBoard Mode Start=%d!\n",StartOffControl);
//        	StartOffControl = true;
//        }




        if(!StartOffControl)
        	continue;
      //  ROS_INFO("StartOffControl=%d\n",StartOffControl);
        switch(c)
        {
        	//need to start offboard mode

        	case KEYCODE_8:
        		x_bias += 0.05;
        		break;
        	case KEYCODE_2:
        		x_bias -= 0.05;
        		break;
        	case KEYCODE_4:
        	    y_bias += 0.05;
        	    break;
        	case KEYCODE_6:
        	    y_bias -= 0.05;
        	    break;
        	case KEYCODE_7:
        		R_bias += 0.05;
        	    break;
        	case KEYCODE_1:
        		R_bias -= 0.05;
        	    break;
        	case KEYCODE_9:
        		rotate_speed_bias += 0.05;
        	    break;
        	case KEYCODE_3:
        		rotate_speed_bias -= 0.05;
        	    break;
        	case KEYCODE_5:
        	    z_bias += 0.05;
        	    break;
        	case KEYCODE_0:
        	    z_bias -= 0.05;
        	    break;
            case KEYCODE_W:
            	//rotate_v += 0.05;
            	x_sp -= 0.2;
                dirty = true;
                break;
            case KEYCODE_S:
            	//rotate_v -= 0.05;
            	x_sp += 0.2;
                dirty = true;
                break;
            case KEYCODE_A:

            	y_sp -=0.2;
                dirty = true;
                break;
            case KEYCODE_D:

            	y_sp +=0.2;
                dirty = true;
                break;

            case KEYCODE_W_CAP:
                dirty = true;
                break;
            case KEYCODE_S_CAP:

                dirty = true;
                break;
            case KEYCODE_A_CAP:

                dirty = true;
                break;
            case KEYCODE_D_CAP:

                dirty = true;
                break;
            case KEYCODE_BEGIN:
            	x_bias = 0;
                y_bias = 0;
            	z_bias = 0;
            	R_bias = 0;
            	rotate_speed_bias = 0;
            default:

                dirty = false;
        }

        if(c)
        {
        ROS_INFO("char = %d\n",(int)c);
        }

        if(reset)
        {
        	x_bias = 0;
        	y_bias = 0;
        	z_bias = 0;
        	R_bias = 0;
        	rotate_speed_bias = 0;
        	reset = false;

        }
        vel_modify_keyboard.twist.angular.x = x_bias;
        vel_modify_keyboard.twist.angular.y = y_bias;
        vel_modify_keyboard.twist.angular.z = z_bias;

        vel_modify_keyboard.twist.linear.x = R_bias;
        vel_modify_keyboard.twist.linear.y = rotate_speed_bias;

        vel_modify_keyboard.header.stamp = ros::Time::now();

        pose_sp_keyboard.pose.position.x =x_sp;
        pose_sp_keyboard.pose.position.y =y_sp;
		pose_sp_keyboard.pose.position.z =z_sp;


		pose_sp_keyboard.header.stamp = ros::Time::now();

	//	printf("X_set= %f Y_set= %f Z_set= %f \n", x_sp,y_sp,z_sp);
	//	pose_set_pub.publish(pose_sp_keyboard);
		if((co++)%100==1)
		{
				printf("x_b= %f  y_b= %f z_b= %f \n",x_bias,y_bias,z_bias);
		}

		if((co)%100==51)
		{
				printf("R_bias= %f  rotate_speed_bias= %f \n",R_bias,rotate_speed_bias);
	    }


			vel_modify_set_pub.publish(vel_modify_keyboard);


    }
}
